#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include "driverlib/gpio.h"
#include "inc/hw_memmap.h"
#include "driverlib/pwm.h"
#include "driverlib/adc.h"
#include "driverlib/uart.h"
#include "driverlib/sysctl.h"
#include "my_cmd_lib.h"



double kp = 120;
double kpUpper = 400;
double kd = 600;
double kdUpper = 2000;
double ki = 1.0;

double pre_err = 0;
double accu_err = 0;

double Q_gain = 0.7;
double Q_upper = 0.01;
double Q_lower = 0.15;
double R = 0.03;
double distance_hat0 = 10;
double P0 = 18.75;
double Q;
double P;
double distance_hat;

uint32_t defaultR_v = 1000;
uint32_t defaultL_v = 1000;

uint32_t deltaPIDnega = 400;
uint32_t deltaPIDposi = 700;


uint32_t uturnlv = 700;
uint32_t uturnrv = 1000;
uint32_t rturnlv = 1000;
uint32_t rturnrv = 535;

uint32_t thinLower = 2;
uint32_t thinUpper = 25;
uint32_t thickLower = 30;
uint32_t thickUpper = 80;

double uTurn_thresIn = 9;
double rTurn_thresIn = 23;
double uTurn_thresOut = 16.5;
double rTurn_thresOut = 14.87;

volatile bool TR = 0;
volatile bool UT = 0;

extern uint32_t timestamp;



void uint2str(char* str, uint32_t num){
	if(num == 0){
		str[0] = '0';
		str[1] = '\0';
		return;
	}
	int numberofnum = 0;
	int copynum = num;
	while (copynum){
		numberofnum++;
		copynum /= 10;
	}
	str[numberofnum] = '\0';
	while (numberofnum){
		str[numberofnum-1] = num % 10 + '0';
		num /= 10;
		numberofnum--;
	}
}

void double2str(char* str, double num){
	if (num < 0) {
		str[0] = '-';
		str++;
		num = -num;
	}
	char temp[5];
	int integer = num;
	uint2str(temp, integer);
	strcpy(str, temp);
	strcat(str, ".");
	temp[0] = (int)(num * 10) % 10 + '0';
	temp[1] = '\0';
	strcat(str, temp);
	temp[0] = (int)(num * 100) % 10 + '0';
	temp[1] = '\0';
	strcat(str, temp);
}


void UARTSend(const uint8_t *pui8Buffer, const uint32_t UART_BASE)
{
    while(*pui8Buffer)
    {
    	UARTCharPut(UART_BASE, *pui8Buffer++);
    }
}

double kalman_filter(double mea_distance){
	double Kk, eMeaHat;
	eMeaHat = mea_distance - distance_hat;
	if (fabs(eMeaHat) >= 2) {
		distance_hat = mea_distance;
		return distance_hat;
	}
	P = P + Q;
	Kk = P / (P + R);
	distance_hat = distance_hat + Kk * eMeaHat;
	P = (1 - Kk) * P;
	if (fabs(TARGET - distance_hat) <= 0.5){
		Q = Q * Q_gain;
		if (Q < Q_lower / 1000) Q = Q_lower / 1000;
	}
	else{
		Q = Q / Q_gain;
		if (Q > Q_upper) Q = Q_upper;
	}
	return distance_hat;
}

double PID_adjustment(double error){
	accu_err += error;
//	double adjustment = ((kpUpper - kp) / 5 * fabs(error) + kp)*error +
//			((kdUpper - kd) / 5 * fabs(error) + kd) *(error - pre_err) + ki*accu_err;
	double adjustment = kp * error + kd *(error - pre_err) + ki * accu_err;
	pre_err = error;
	return adjustment;
}

void robot_uturn(double distance_cmF){
	UT = 1;
	GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_3 | GPIO_PIN_2, GPIOD_F);
	GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7 | GPIO_PIN_6, GPIOA_B);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, uturnlv);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, uturnrv);
	if (distance_cmF > uTurn_thresOut) UT = 0;
}

void robot_TR(double distance_cmR){
	TR = 1;
	GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_3 | GPIO_PIN_2, GPIOD_F);
	GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7 | GPIO_PIN_6, GPIOA_F);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, rturnlv);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, rturnrv);
	if (distance_cmR < rTurn_thresOut) TR = 0;
}


void PID_followWall(double error){
	double pid_adj = PID_adjustment(error);
	if (pid_adj < -(double)deltaPIDnega) pid_adj = -(double)deltaPIDnega;
	if (pid_adj > deltaPIDposi) pid_adj = deltaPIDposi;
	int New_pulsewidthr = defaultR_v + pid_adj;
	int New_pulsewidthl = defaultL_v - pid_adj;
	if (New_pulsewidthr > 1000) New_pulsewidthr = 1000;
	if (New_pulsewidthr < 1) New_pulsewidthr = 1;
	if (New_pulsewidthl > 1000) New_pulsewidthl = 1000;
	if (New_pulsewidthl < 1) New_pulsewidthl = 1;
	GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_3 | GPIO_PIN_2, GPIOD_F);
	GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7 | GPIO_PIN_6, GPIOA_F);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, New_pulsewidthl);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, New_pulsewidthr);
}


